#include <Servo.h>
#include <CheapStepper.h>

//define I/O ports
#define inputSelector A5
#define limitSwitch A4
#define spindleRotation 2
#define LED1 3
#define LED2 4
#define startSwitch 5
#define stopSwitch 6
#define spindleServo 7
#define UN2003IN4 9
#define UN2003IN3 10
#define UN2003IN2 11
#define UN2003IN1 12
#define outputVoltage A1
#define potentioMeter A2

//define stepper speed
#define stepperNormalSpeed 2
#define stepperFastSpeed 20
#define dangerLowVoltage 230
#define hysteresis 5
#define stepperMaxTravel 245760
#define holdOffDistance 3000

//define variables
int currentPotPosition = 0;
int currentVoltage = 0;
int lowerHysteresis = 0;
int higherHysteresis = 0;
int startLocation = 0;
int shortCircuitLocation = 0;
bool startLocationSet = false;
bool moveBackward = true;
bool moveUp = false;
bool moveDown = false;
bool starT = false;
bool stoP = false;
int stepsLeft = 0;
static unsigned int stateMachine = 0;  // Initialize the state as "inactive"

//Setup servos
Servo spindle;
CheapStepper stepper (12,11,10,9);



// Simple macros to declare debounce input pins.. Saves time typing.
#define DEBOUNCE_DECLARE(p)            \
bool p ## _state = 0;                  \
int p ## _count = 0;

// macro to debounce a pin.
#define debounceCount 10
#define DEBOUNCE(p)  do {              \
if(digitalRead(p) == HIGH) {           \
  p ## _count++;                       \
  if(p ## _count >= debounceCount) {   \
    p ## _state = 1;                   \
  } else {                             \
    p ## _state = 0;                   \
  }                                    \
} else {                               \
  p ## _count = 0;                     \
  p ## _state = 0;                     \
}                                      \
} while(0) /* We use do{ }while(0) because this allows us to put a ";" behind the macro when using it. */

// Declare the variables we use to store the debounced state in.
DEBOUNCE_DECLARE(inputSelector);
DEBOUNCE_DECLARE(spindleRotation);
DEBOUNCE_DECLARE(startSwitch);
DEBOUNCE_DECLARE(stopSwitch);
DEBOUNCE_DECLARE(limitSwitch);

void setup(){

  // Configure I/O
  pinMode(inputSelector, INPUT);
  pinMode(limitSwitch, INPUT);
  pinMode(spindleRotation, INPUT);
  pinMode(LED1, OUTPUT);
  pinMode(LED2, OUTPUT);
  pinMode(startSwitch, INPUT);
  pinMode(stopSwitch, INPUT);
  pinMode(spindleServo, OUTPUT);
  pinMode(UN2003IN1, OUTPUT);
  pinMode(UN2003IN2, OUTPUT);
  pinMode(UN2003IN3, OUTPUT);
  pinMode(UN2003IN4, OUTPUT);
  pinMode(outputVoltage, INPUT);
  pinMode(potentioMeter, INPUT);
  spindle.attach(spindleServo);   // Attach pulse output pin for spindle servo
 // Serial.begin(115200);
}





void loop(){
  // Debounce our input pins.
  DEBOUNCE(stopSwitch);
  DEBOUNCE(startSwitch);
  DEBOUNCE(spindleRotation);
  DEBOUNCE(inputSelector);
  DEBOUNCE(limitSwitch);
  if(inputSelector_state == 1){
    if (startSwitch_state == 1 && stopSwitch_state == 0){
      moveUp = true;
      moveDown = false;
    } else if (stopSwitch_state == 1 && startSwitch_state == 0){
      moveUp = false;
      moveDown = true;
    } else {
      moveUp = false;
      moveDown = false;
    }

  } else
  {
    moveUp = false;
    moveDown = false;
    if(startSwitch_state == 1 && stopSwitch_state == 0){
      starT = true;
      stoP = false;
    } else if (stopSwitch_state == 1 && startSwitch_state == 0){
      starT = false;
      stoP = true;
    }else {
      starT = false;
      stoP = false;
    }
  }
  stepsLeft = stepper.getStepsLeft();



  switch (stateMachine){
    case 0:   // Standby state.
    //Serial.println("Case 0, Standby");
      if(moveUp == 1){
        /*stepper.setRpm(stepperFastSpeed);
        moveBackward = true;
        stepper.newMove (moveBackward, 20);
        stepper.run(); */
        stepper.step(1);
      } else if(moveDown == 1){
        /*stepper.setRpm(stepperFastSpeed);
        moveBackward = false;
        stepper.newMove (moveBackward, 20);
        stepper.run();*/
        stepper.step(0);
      } else{
        stepper.stop();
      }
      if(starT == 1){ // Start switch activated
        digitalWrite(LED1, 1); // Turn on LED
        stepper.setRpm(stepperNormalSpeed); //Set stepper normal speed for regular runtime
        moveBackward = false; // Run forward
        stepper.newMoveTo (moveBackward, stepperMaxTravel); //number of degrees to move forward. (This is full the travel of the ballscrew)
        stateMachine = 1;
      }
      break;
    case 1:   // Run state. The LED is on, wait for instructions to stop.
      //Serial.println("Case 1, RUN");
      //if(spindleRotation_state == 1) {
        //Serial.println("SpindleRotation_state = 1");
      //  spindle.write(180);  // Set spindle rotation at full speed (180=full speed clockwise, 90=stop, 0=full speed counter clockwise)
      //} else{
        //Serial.println("SpindleRotation_state = 0");
      //  spindle.write(90);  //stop spindle rotation
      //}
      currentVoltage = analogRead(outputVoltage); //reads the voltage number from the analog input
      currentPotPosition = analogRead(potentioMeter); //Reads the position of the potentiometer
      lowerHysteresis = (currentPotPosition - hysteresis);  //Sets lower hysteresis
      higherHysteresis = (currentPotPosition + hysteresis); //Sets higher hysteresis

      if(stoP == 1 or limitSwitch_state == 1){ // When this input is high, go to stop
        digitalWrite(LED1, 0);  // Switch off LED.
        stepper.stop(); //stop stepper
        spindle.write(90); //stop spindle rotation
        stateMachine = 2;
        }
      if(higherHysteresis > currentVoltage > lowerHysteresis) { // hold it right there, thats the spot!
        stepper.stop();
        if(startLocationSet == true){
          startLocationSet = true;
          startLocation = stepper.getStep();  //Get the location of the first hysteresis (close to surface)
        }
        } else if(currentVoltage <= dangerLowVoltage) { //Detecting short circuit
        stateMachine = 3;

        } else {
          stepper.newMoveTo (moveBackward, stepperMaxTravel); //number of degrees to move forward. (This is full the travel of the ballscrew)
          stepper.run();
        }
      break;

    case 2:   // Stop state
      //Serial.println("Case 2, STOP");
      moveBackward = true;  //Reverse stepper direction
      stepper.setRpm(stepperFastSpeed); // Set fast step speed for retraction.
      stepper.newMoveTo(moveBackward, 0);    //Set stepper location to start position
      if(stepsLeft == 0){
        stateMachine = 4;
      } else {
        stepper.run();
      }
      break;

    case 3:   // short circut
      //Serial.println("Case 3, Short circuit");
      stepper.stop();  //stop stepper
      shortCircuitLocation = stepper.getStep(); //Get the location of the short circuit
      moveBackward = true; //change direction of movement
      stepper.setRpm(stepperFastSpeed); //sets fast speed for stepper
      stepper.newMoveTo(moveBackward, startLocation);  //makes stepper move to start position
      stateMachine = 5;
      break;

    case 4:   // Reset variables & return to standby
      //Serial.println("Case 4, Reset");
      //reset all variables
      stateMachine = 0;
      break;

    case 5:  // short circuit backwards movement
    //Serial.println("Case 5, Short circuit back");
      if(stepsLeft == 0){ //if movement is done
        stepper.stop();  //stop stepper
        moveBackward = false; //change direction of movement
        stepper.newMoveTo(moveBackward, shortCircuitLocation - holdOffDistance); //Move back close to the point of short circuit
        stateMachine = 6; //set stateMachine to short circuit forward movement
      } else {
        stepper.run();  //continue running stepper
      }
      break;

    case 6:  // short circuit forwards movement
      //Serial.println("Case 6, Short circuit forward");
      if(stepsLeft == 0){ //if movement is done
        moveBackward = false;
        stepper.newMoveTo(moveBackward, stepperMaxTravel);
        stateMachine = 1; //set stateMachine to running
        stepper.setRpm(stepperNormalSpeed);
      } else{
        stepper.run();  //Continue running stepper
      }
      break;

    default:  // Unknown state!
      //Serial.println("Case default, unknown state");
      stateMachine = 0;  // Go to standby.
      break;
  }
}